import sensor, image, time, pyb

from pyb import Servo
from pyb import LED
from pyb import UART

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time = 2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
clock = time.clock()

s1 = Servo(1) #P7
s1.angle(55) #恢复角度

uart = UART(3, 9600)

led1 = pyb.LED(1) #红
led2 = pyb.LED(3) #蓝
led3 = pyb.LED(2) #绿

R = [80, 60, 50, 50] #设定感兴趣识别区域

a=0
b=0
c=0
d=0
n=0
flag=0
seka=1
qiqiu=0
data=bytearray([0x01])


#led1.on() #红
#led2.on() #蓝
#led3.on() #绿

while(True):
   if uart.any():
      a = uart.readline()
      #print(a)
   if a == b'A' and d==0:
      #print(123)
      if flag==0:
        clock.tick()
        img = sensor.snapshot()
        img.draw_rectangle(R)#画出识别区域(矩形)

        statistics=img.get_statistics(roi=R) #作用：获取阈值
        color_l_min=statistics.l_min()
        color_a_min=statistics.a_min()
        color_b_min=statistics.b_min()
        color_l_max=statistics.l_max()
        color_a_max=statistics.a_max()
        color_b_max=statistics.b_max()

        #print(color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max) #用于读取阈值
        if -2<color_a_min<2 and 40<color_a_max<85  and -30<color_b_min<4 and  20<color_b_max<70:#红色
           Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
           blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
           for blob in blobs:
               if blob.pixels()>=2400:#如果色块像素数量小于这个值，会被过滤掉，防止识别错误
                  seka = 2
                  flag = 1
                  c = 1
                  led1.on()
        elif -15<color_a_min<15 and -15<color_a_max<65  and -100<color_b_min<-30 and 0<color_b_max<4:#蓝色
              Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
              blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
              for blob in blobs:
                  if blob.pixels()>=2400:
                     seka = 3
                     flag = 1
                     c = 1
                     led2.on()
        elif -75<color_a_min<-25 and -2<color_a_max<10  and -10<color_b_min<15 and 10<color_b_max<75:#绿色
              Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
              blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
              for blob in blobs:
                  if blob.pixels()>=2400:
                     seka = 4
                     flag = 1
                     c = 1
                     led3.on()

      if c==1 and flag==1:
        time.sleep_ms(1300)

      if flag==1:
        clock.tick()
        img = sensor.snapshot()
        img.draw_rectangle(R)#画出识别区域(矩形)

        statistics=img.get_statistics(roi=R) #作用：获取阈值
        color_l_min=statistics.l_min()
        color_a_min=statistics.a_min()
        color_b_min=statistics.b_min()
        color_l_max=statistics.l_max()
        color_a_max=statistics.a_max()
        color_b_max=statistics.b_max()

        if  -2<color_a_min<2 and 40<color_a_max<85  and -30<color_b_min<4 and  20<color_b_max<70 and seka == 2:#红色
            Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
            blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
            for blob in blobs:
                if blob.pixels()>=2400:
                   uart.write(data)
                   qiqiu = 2
                   flag = 2
                   led2.on()
                   led3.on()
        elif -25<color_a_min<25 and -25<color_a_max<75  and -110<color_b_min<-20 and 0<color_b_max<4 and seka == 3:#蓝色
              Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
              blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
              for blob in blobs:
                  if blob.pixels()>=2400:
                     uart.write(data)
                     qiqiu = 3
                     flag = 2
                     led1.on()
                     led3.on()
        elif -75<color_a_min<-25 and -2<color_a_max<10  and -10<color_b_min<15 and 10<color_b_max<75 and seka == 4:#绿色
              Getcolour_thresholds = (color_l_min,color_l_max,color_a_min,color_a_max,color_b_min,color_b_max)
              blobs = img.find_blobs([Getcolour_thresholds],ROI=R)
              for blob in blobs:
                  if blob.pixels()>=2400:
                     uart.write(data)
                     qiqiu = 4
                     flag = 2
                     led1.on()
                     led2.on()
      if qiqiu == seka:
         s1.angle(-90)
         time.sleep_ms(2000)
         s1.angle(55)
         flag = 3
         d = 1
